#include "EEPROM.h"
#include <string.h>


volatile float PID_data_array[4][3] = {
    {0, 0, 0}, 
    {0, 0, 0}, // Gray PID
    {0, 0, 0},  // Servo PID
	{0, 0, 0}// Yaw PID
};

uint8_t spi_read_write_byte(uint8_t dat)
{
        uint8_t data = 0;

        //发送数据
        DL_SPI_transmitData8(SPI_LCD_INST ,dat);
        //等待SPI总线空闲
        while(DL_SPI_isBusy(SPI_LCD_INST ));
        //接收数据
        data = DL_SPI_receiveData8(SPI_LCD_INST );
        //等待SPI总线空闲
        while(DL_SPI_isBusy(SPI_LCD_INST ));

        return data;
}

//读取芯片ID
//返回值如下:
//0XEF13,表示芯片型号为W25Q80
//0XEF14,表示芯片型号为W25Q16
//0XEF15,表示芯片型号为W25Q32
//0XEF16,表示芯片型号为W25Q64
//0XEF17,表示芯片型号为W25Q128
//读取设备ID
uint16_t W25Q128_readID(void)
{
    uint16_t  temp = 0;
    //将CS端拉低为低电平
    SPI_CS(0);
    //发送指令90h
    spi_read_write_byte(0x90);//发送读取ID命令
    //发送地址  000000H
    spi_read_write_byte(0x00);
    spi_read_write_byte(0x00);
    spi_read_write_byte(0x00);

    //接收数据
    //接收制造商ID
    temp |= spi_read_write_byte(0xFF)<<8;
    //接收设备ID
    temp |= spi_read_write_byte(0xFF);
    //恢复CS端为高电平
    SPI_CS(1);
    //返回ID
    return temp;
}

//发送写使能
void W25Q128_write_enable(void)
{
     
    //拉低CS端为低电平
    SPI_CS(0);
    //发送指令06h
    spi_read_write_byte(0x06);
    //拉高CS端为高电平
    SPI_CS(1);
     
}

void W25Q128_wait_busy(void)
{
	unsigned char byte = 0;
	do
	 {
             
			//拉低CS端为低电平
			SPI_CS(0);
			//发送指令05h
			spi_read_write_byte(0x05);
			//接收状态寄存器值
			byte = spi_read_write_byte(0Xff);
			//恢复CS端为高电平
			SPI_CS(1);
             
	 //判断BUSY位是否为1 如果为1说明在忙，重新读写BUSY位直到为0
	 }while( ( byte & 0x01 ) == 1 );
}

/**********************************************************
 * 函 数 名 称：W25Q128_erase_sector
 * 函 数 功 能：擦除一个扇区
 * 传 入 参 数：addr=擦除的扇区号
 * 函 数 返 回：无
 * 作       者：LC
 * 备       注：addr=擦除的扇区号，范围=0~15
**********************************************************/
void W25Q128_erase_sector(uint32_t addr)
{
	//计算扇区号，一个扇区4KB=4096
	addr *= 4096;
	W25Q128_write_enable();  //写使能
	W25Q128_wait_busy();     //判断忙，如果忙则一直等待
     
	//拉低CS端为低电平
	SPI_CS(0);
	//发送指令20h
	spi_read_write_byte(0x20);
	//发送24位扇区地址的高8位
	spi_read_write_byte((uint8_t)((addr)>>16));
	//发送24位扇区地址的中8位
	spi_read_write_byte((uint8_t)((addr)>>8));
	//发送24位扇区地址的低8位
	spi_read_write_byte((uint8_t)addr);
	//恢复CS端为高电平
	SPI_CS(1);
	//等待擦除完成
     
	W25Q128_wait_busy();
}

/**********************************************************
 * 函 数 名 称：W25Q128_write
 * 函 数 功 能：写数据到W25Q128进行保存
 * 传 入 参 数：buffer=写入的数据内容        addr=写入地址        numbyte=写入数据的长度
 * 函 数 返 回：无
 * 作       者：LC
 * 备       注：无
**********************************************************/
void W25Q128_write(uint8_t* buffer, uint32_t addr, uint16_t numbyte)
{
    unsigned int i = 0;
    //擦除扇区数据
    //W25Q128_erase_sector(addr/4096);
    //写使能
    W25Q128_write_enable();
    //忙检测
    W25Q128_wait_busy();
    //写入数据
     
    //拉低CS端为低电平
    SPI_CS(0);
    //发送指令02h
    spi_read_write_byte(0x02);
    //发送写入的24位地址中的高8位
    spi_read_write_byte((uint8_t)((addr)>>16));
    //发送写入的24位地址中的中8位
    spi_read_write_byte((uint8_t)((addr)>>8));
    //发送写入的24位地址中的低8位
    spi_read_write_byte((uint8_t)addr);
    //根据写入的字节长度连续写入数据buffer
    for(i=0;i<numbyte;i++)
    {
        spi_read_write_byte(buffer[i]);
    }
    //恢复CS端为高电平
    SPI_CS(1);
    //忙检测
     
    W25Q128_wait_busy();
}

/**********************************************************
 * 函 数 名 称：W25Q128_read
 * 函 数 功 能：读取W25Q128的数据
 * 传 入 参 数：buffer=读出数据的保存地址  read_addr=读取地址   read_length=读去长度
 * 函 数 返 回：无
 * 作       者：LC
 * 备       注：无
**********************************************************/
void W25Q128_read(uint8_t* buffer,uint32_t read_addr,uint16_t read_length)
{
	uint16_t i;
     
	//拉低CS端为低电平
	SPI_CS(0);
	//发送指令03h
	spi_read_write_byte(0x03);
	//发送24位读取数据地址的高8位
	spi_read_write_byte((uint8_t)((read_addr)>>16));
	//发送24位读取数据地址的中8位
	spi_read_write_byte((uint8_t)((read_addr)>>8));
	//发送24位读取数据地址的低8位
	spi_read_write_byte((uint8_t)read_addr);
	//根据读取长度读取出地址保存到buffer中
	for(i=0;i<read_length;i++)
	{
		buffer[i]= spi_read_write_byte(0XFF);
	}
	//恢复CS端为高电平
	SPI_CS(1);
     
}


/* void EEPROM_WriteFloat(uint32_t addr, float value)
{
    uint8_t data[4];
    uint8_t cmd[4];
    memcpy(data, &value, sizeof(value));

    // 写入前先擦除所在扇区（4KB 对齐）
    uint32_t sector_addr = addr & ~(4096 - 1);
    W25Q128_erase_sector(sector_addr);

    W25Q128_write_enable(); // 写使能

    cmd[0] = 0x02; // 页写入命令
    cmd[1] = (addr >> 16) & 0xFF;
    cmd[2] = (addr >> 8) & 0xFF;
    cmd[3] = addr & 0xFF;

     
    SPI_CS(0); // 拉低片选

    for (int i = 0; i < 4; i++)
        spi_read_write_byte(cmd[i]); // 发送命令 + 地址

    for (int i = 0; i < 4; i++)
        spi_read_write_byte(data[i]); // 写入数据

    SPI_CS(1); // 拉高片选
     
    W25Q128_wait_busy(); // 等待写完成
} */void EEPROM_WriteFloat(uint32_t addr, float value)
{
    uint8_t raw[4];
    memcpy(raw, &value, sizeof(float));

    W25Q128_erase_sector(addr / 4096);  // 只擦除一次
    W25Q128_write(raw, addr, 4);        // 写入4字节
}


/* float EEPROM_ReadFloat(uint32_t addr)
{
    float value;
    uint8_t data[4];
    uint8_t cmd[4];

    cmd[0] = 0x03; // 读取命令
    cmd[1] = (addr >> 16) & 0xFF;
    cmd[2] = (addr >> 8) & 0xFF;
    cmd[3] = addr & 0xFF;

     
    SPI_CS(0); // 片选拉低，开始通信

    for (int i = 0; i < 4; i++)
        spi_read_write_byte(cmd[i]);  // 发送命令 + 地址

    for (int i = 0; i < 4; i++)
        data[i] = spi_read_write_byte(0xFF); // 接收4字节数据

    SPI_CS(1); // 通信结束
     
    memcpy(&value, data, sizeof(value));
    return value;
}
 */
float EEPROM_ReadFloat(uint32_t addr)
{
	__disable_irq();
    uint8_t raw[4];
    W25Q128_read(raw, addr, 4);

    float value;
    memcpy(&value, raw, 4);
	__enable_irq();
    return value;
		
}


void W25QXX_first_Init(void) //第一次写入EEPROM,擦除2048个扇区，然后将默认值赋值进去
{
    __disable_irq(); // 如果需要关中断，可以取消注释
    uart0_printf("W25QXX first initialization...\n");
    //擦除2048个扇区
    //W25QXX_erase_all(); // 擦除前2048个扇区
    //擦除前200扇区
    for (uint32_t i = 0; i < 200; i++)
    {
       uart0_printf("Erasing sector %d...\n", i);
        W25Q128_erase_sector(i * 4096); // 每个扇区4K字节
    }
    EEPROM_WriteFloat(FUCK_KP_ADDR, 0.0f);
    EEPROM_WriteFloat(FUCK_KI_ADDR, 0.0f);
    EEPROM_WriteFloat(FUCK_KD_ADDR, 0.0f);

    //将默认值赋值进去
    EEPROM_WriteFloat(YAW_KP_ADDR, Yaw_PID.Kp);
    EEPROM_WriteFloat(YAW_KI_ADDR, Yaw_PID.Ki);
    EEPROM_WriteFloat(YAW_KD_ADDR, Yaw_PID.Kd);

    EEPROM_WriteFloat(GRAY_KP_ADDR, GW_gray_PID.Kp);
    EEPROM_WriteFloat(GRAY_KI_ADDR, GW_gray_PID.Ki);
    EEPROM_WriteFloat(GRAY_KD_ADDR, GW_gray_PID.Kd);

    EEPROM_WriteFloat(SERVO_KP_ADDR, Servox_PID.Kp);
    EEPROM_WriteFloat(SERVO_KI_ADDR, Servox_PID.Ki);
    EEPROM_WriteFloat(SERVO_KD_ADDR, Servox_PID.Kd);
    __enable_irq(); // 如果需要关中断，可以取消注释
    uart0_printf("First initialization complete. Default values written to EEPROM.\n");
}


void EEPROM_WriteAllPIDData(void)
{           //关中断
    __disable_irq(); // 如果需要关中断，可以取消注释
    // 将PID数据写入EEPROM
	  //uart0_printf("%.2f \n",PID_data_array[3][0]);
    delay_ms(10);

    EEPROM_WriteFloat(FUCK_KP_ADDR, PID_data_array[3][0]);
    EEPROM_WriteFloat(FUCK_KI_ADDR, PID_data_array[3][1]);
    EEPROM_WriteFloat(FUCK_KD_ADDR, PID_data_array[3][2]);

    EEPROM_WriteFloat(YAW_KP_ADDR, PID_data_array[3][0]);
    EEPROM_WriteFloat(YAW_KI_ADDR, PID_data_array[3][1]);
    EEPROM_WriteFloat(YAW_KD_ADDR, PID_data_array[3][2]);

    EEPROM_WriteFloat(GRAY_KP_ADDR, PID_data_array[1][0]);
    EEPROM_WriteFloat(GRAY_KI_ADDR, PID_data_array[1][1]);
    EEPROM_WriteFloat(GRAY_KD_ADDR, PID_data_array[1][2]);

    EEPROM_WriteFloat(SERVO_KP_ADDR, PID_data_array[2][0]);
    EEPROM_WriteFloat(SERVO_KI_ADDR, PID_data_array[2][1]);
    EEPROM_WriteFloat(SERVO_KD_ADDR, PID_data_array[2][2]);




    //开启中断
    __enable_irq(); // 如果需要关中断，可以取消注释
}//写入所有 PID 数据

void EEPROM_ReadAllPIDData(void)
{
    PID_data_array[3][0] = EEPROM_ReadFloat(FUCK_KP_ADDR);
    PID_data_array[3][1] = EEPROM_ReadFloat(FUCK_KI_ADDR);
    PID_data_array[3][2] = EEPROM_ReadFloat(FUCK_KD_ADDR);

    PID_data_array[3][0] = EEPROM_ReadFloat(YAW_KP_ADDR);
    PID_data_array[3][1] = EEPROM_ReadFloat(YAW_KI_ADDR);
    PID_data_array[3][2] = EEPROM_ReadFloat(YAW_KD_ADDR);



    Yaw_PID.Kp = PID_data_array[3][0];
    Yaw_PID.Ki = PID_data_array[3][1];
    Yaw_PID.Kd = PID_data_array[3][2];

    PID_data_array[1][0] = EEPROM_ReadFloat(GRAY_KP_ADDR);
    PID_data_array[1][1] = EEPROM_ReadFloat(GRAY_KI_ADDR);
    PID_data_array[1][2] = EEPROM_ReadFloat(GRAY_KD_ADDR);
    GW_gray_PID.Kp = PID_data_array[1][0];
    GW_gray_PID.Ki = PID_data_array[1][1];
    GW_gray_PID.Kd = PID_data_array[1][2];


    PID_data_array[2][0] = EEPROM_ReadFloat(SERVO_KP_ADDR);
    PID_data_array[2][1] = EEPROM_ReadFloat(SERVO_KI_ADDR);
    PID_data_array[2][2] = EEPROM_ReadFloat(SERVO_KD_ADDR);
    Servox_PID.Kp = PID_data_array[2][0];
    Servox_PID.Ki = PID_data_array[2][1];
    Servox_PID.Kd = PID_data_array[2][2];

    Servoy_PID.Kp = PID_data_array[2][0];
    Servoy_PID.Ki = PID_data_array[2][1];
    Servoy_PID.Kd = PID_data_array[2][2];
}//读取所有 PID 数据


void EEPROM_Init(void)
{
    EEPROM_ReadAllPIDData();
}//初始化使用 EEPROM 数据

void EEPROM_Save(void)
{
    EEPROM_WriteAllPIDData();
}//保存 EEPROM 数据


void test_eeprom_rw(void)
{
PID_data_array[3][0] = 3.10f;
EEPROM_WriteFloat(YAW_KP_ADDR, PID_data_array[3][0]);

float tmp = EEPROM_ReadFloat(YAW_KP_ADDR);
uart0_printf("Write: %.5f, Read: %.5f\n", PID_data_array[3][0], tmp);
}//test